#include<wiringPi.h>
#include<pthread.h>
#include<stdlib.h>
#include<stdio.h>
#include<string.h>
#include<math.h>
#include<sys/time.h>

#define pwm_pin1 1
#define pwm_pin2 26
#define pwm_pin3 23
#define pwm_pin4 24

#define Device_Address 0x68	/*Device Address/Identifier for MPU6050*/

#define PWR_MGMT_1   0x6B
#define SMPLRT_DIV   0x19
#define CONFIG       0x1A
#define GYRO_CONFIG  0x1B
#define INT_ENABLE   0x38
#define ACCEL_XOUT_H 0x3B
#define ACCEL_YOUT_H 0x3D
#define ACCEL_ZOUT_H 0x3F
#define GYRO_XOUT_H  0x43
#define GYRO_YOUT_H  0x45
#define GYRO_ZOUT_H  0x47

#define X_ACCEL_OFFSET -600 
#define Y_ACCEL_OFFSET -100 
#define Z_ACCEL_OFFSET 2900 
#define X_GYRO_OFFSET 32 
#define Y_GYRO_OFFSET -11 
#define Z_GYRO_OFFSET 1 

typedef struct Angle
{
		double X_Angle;
		double Y_Angle;
		double Z_Angle;
}MPU6050_Angle;

int pwm1 = 400;int pwm2 = 400;int pwm3 = 400;int pwm4 = 400;
int pyPwm1 = 0;int pyPwm2 = 0;int pyPwm3 = 0;int pyPwm4 = 0;
int stack; int up = 8; int no = 5; int dowm = 2;  int off = 100;
int fd;
MPU6050_Angle data;

void MPU6050_Init(){
	wiringPiI2CWriteReg8(fd, SMPLRT_DIV, 0x07);	/* Write to sample rate register */
	wiringPiI2CWriteReg8(fd, PWR_MGMT_1, 0x01);	/* Write to power management register */
	wiringPiI2CWriteReg8(fd, CONFIG, 0);		/* Write to Configuration register */
	wiringPiI2CWriteReg8(fd, GYRO_CONFIG, 24);	/* Write to Gyro Configuration register */
	wiringPiI2CWriteReg8(fd, INT_ENABLE, 0x01);	/*Write to interrupt enable register */
} 

int64_t getCurrentTime() {
	struct timeval tv;
    	gettimeofday(&tv, NULL);
	return ((unsigned long long)tv.tv_sec * 1000000 + (unsigned long long)tv.tv_usec);
}

void pwm_maker1(int *value)
{
	pwmWrite(pwm_pin1,1000);
	delay(1000);
	pwmWrite(pwm_pin1,0);
	while(1){
		int64_t lastTime = getCurrentTime();
		pwmWrite(pwm_pin1,pwm1+20+pyPwm1);
		int64_t currentTime = getCurrentTime();
		int time = currentTime - lastTime;
		if (time > 5) {
			usleep(1);
		}
		else {
			usleep(5 - time);
		}
	}
}

void pwm_maker2(int *value)
{
	pwmWrite(pwm_pin2,1000);
	delay(1000);
	pwmWrite(pwm_pin2,0);
	while(1){
		int64_t lastTime = getCurrentTime();
		pwmWrite(pwm_pin2,pwm2+20+pyPwm2);
		int64_t currentTime = getCurrentTime();
		int time = currentTime - lastTime;
		if (time > 5) {
			usleep(1);
		}
		else {
			usleep(5 - time);
		}
	}
}

void pwm_maker3(int *value)
{
	pwmWrite(pwm_pin3,1000);
	delay(1000);
	pwmWrite(pwm_pin3,0);
	while(1){
		int64_t lastTime = getCurrentTime();
		pwmWrite(pwm_pin3,pwm3+pyPwm1);
		int64_t currentTime = getCurrentTime();
		int time = currentTime - lastTime;
		if (time > 5) {
			usleep(1);
		}
		else {
			usleep(5 - time);
		}
	}
}

void pwm_maker4(int *value)
{
	pwmWrite(pwm_pin4,1000);
	delay(1000);
	pwmWrite(pwm_pin4,0);
	while(1){
		int64_t lastTime = getCurrentTime();
		pwmWrite(pwm_pin4,pwm4+pyPwm4);
		int64_t currentTime = getCurrentTime();
		int time = currentTime - lastTime;
		if (time > 5) {
			usleep(1);
		}
		else {
			usleep(5 - time);
		}
	}
}

void pwm_maker5()
{
	while(1){
		int64_t lastTime = getCurrentTime();
		MU6050_Get_Angle(&data);  // 计算三轴倾角         
        //printf("X_Angle = %lf° ", data.X_Angle);
        //printf("Y_Angle = %lf° \n", data.Y_Angle);
        pyPwm1 = 0 ; pyPwm2 = 0 ; pyPwm3 = 0; pyPwm4 = 0;
        if(data.X_Angle<90.5){
			//pwm1 up , need add pwm4 value;
			int a = (int) (data.X_Angle - 90.5);
			pyPwm4 = abs(a)+10;
		}else if(data.X_Angle>90.5){
			//pwm4 up , need add pwm1 value;
			int a = (int) (data.X_Angle - 90.5);
			pyPwm1 = abs(a)+10;
		}
		
		if(data.Y_Angle<90.5){
			//pwm2 up , need add pwm3 value;
			int a = (int) (data.Y_Angle - 90.5);
			pyPwm3 = abs(a)+10;
		}else if(data.Y_Angle>90.5){
			//pwm3 up , need add pwm2 value;
			int a = (int) (data.Y_Angle - 90.5);
			pyPwm2 = abs(a)+10;
		}
		int64_t currentTime = getCurrentTime();
		int time = currentTime - lastTime;
		if (time > 5) {
			usleep(1);
		}
		else {
			usleep(5 - time);
		}
	}
}

void initDev(){
	pinMode(pwm_pin1,PWM_OUTPUT);
	pinMode(pwm_pin2,PWM_OUTPUT);
	pinMode(pwm_pin3,PWM_OUTPUT);
	pinMode(pwm_pin4,PWM_OUTPUT);
}

void initPwm(){
	pwmSetClock(48);
	pwmSetMode(PWM_MODE_MS);
	pwmSetRange(1000);
}

void initPthread(int pwm_value1,int pwm_value2,int pwm_value3,int pwm_value4){
	pthread_t pth1;
	pthread_t pth2;
	pthread_t pth3;
	pthread_t pth4;
	pthread_t pth5;
	pthread_create(&pth1, NULL, (void *)  &pwm_maker1, (void * (*)(void *)) &pwm_value1);
	pthread_create(&pth2, NULL, (void *)  &pwm_maker2, (void * (*)(void *)) &pwm_value2);
	pthread_create(&pth3, NULL, (void *)  &pwm_maker3, (void * (*)(void *)) &pwm_value3);
	pthread_create(&pth4, NULL, (void *)  &pwm_maker4, (void * (*)(void *)) &pwm_value4);
	pthread_create(&pth5, NULL, (void *)  &pwm_maker5, NULL);
}

short read_raw_data(int addr){
	short high_byte,low_byte,value;
	high_byte = wiringPiI2CReadReg8(fd, addr);
	low_byte = wiringPiI2CReadReg8(fd, addr+1);
	value = (high_byte << 8) | low_byte;
	return value;
}

void MU6050_Get_Angle(MPU6050_Angle *data)
{
    data->X_Angle = acos((read_raw_data(ACCEL_XOUT_H) + X_ACCEL_OFFSET) / 16384.0);
    data->Y_Angle = acos((read_raw_data(ACCEL_YOUT_H) + Y_ACCEL_OFFSET) / 16384.0);
    data->Z_Angle = acos((read_raw_data(ACCEL_ZOUT_H) + Z_ACCEL_OFFSET) / 16384.0);
    
    data->X_Angle = data->X_Angle * 57.29577 + 6.0;
    data->Y_Angle = data->Y_Angle * 57.29577 + 4.0;
    data->Z_Angle = data->Z_Angle * 57.29577;
}

int main(void){
	wiringPiSetup();
	fd = wiringPiI2CSetup(Device_Address);   /*Initializes I2C with device Address*/
	MPU6050_Init();		 /* Initializes MPU6050 */
	initDev();
	initPwm();
	initPthread(pwm1,pwm2,pwm3,pwm4);
	while(1){
		printf("写入命令\n");
		scanf("%d",&stack);
		if(stack == 8){
			printf("up\n");
			pwm1 +=10; pwm2 +=10; pwm3 +=10; pwm4 +=10;
		}else if(stack == 2){
			printf("dowm\n");
			pwm1 -=10; pwm2 -=10; pwm3 -=10; pwm4 -=10;
		}else if(stack == 5){
			printf("prepare\n");
			int pwm = 430;
			pwm1 = pwm; pwm2 = pwm; pwm3 = pwm; pwm4 = pwm;
		}else if(stack == 50){
			printf("up air\n");
			pwm1 =580; pwm2 =580; pwm3 =580; pwm4 =580;
		}else if(stack == 0){
			printf("stop \n");
			int pwm = 350;
			pwm1 =pwm; pwm2 =pwm; pwm3 =pwm; pwm4 =pwm;
		}else if(stack == 100){
			exit(1);
		}
		printf("pwm1:%d,pwm2:%d,pwm3:%d,pwm4:%d\n",pwm1,pwm2,pwm3,pwm4);
		delay(1000);
	}
	return 0;
}
